<!-- This comment will put IE 6, 7 and 8 in quirks mode -->
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<title>include/shark/ObjectiveFunctions/Benchmarks/PoleSimulators/DoublePole.h Source File</title>
<script type="text/javaScript" src="search/search.js"></script>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<script src="https://polyfill.io/v3/polyfill.min.js?features=es6"></script>
<script id="MathJax-script" async src="https://cdn.jsdelivr.net/npm/mathjax@3.0.1/es5/tex-mml-chtml.js"></script>
<script src="../../mlstyle.js"></script>
<link href="../css/besser.css" rel="stylesheet" type="text/css"/>
</head>
<!-- pretty cool: each body gets an id tag which is the basename of the web page  -->
<!--              and allows for page-specific CSS. this is client-side scripted, -->
<!--              so the id will not yet show up in the served source code -->
<script type="text/javascript">
    jQuery(document).ready(function () {
        var url = jQuery(location).attr('href');
        var pname = url.substr(url.lastIndexOf("/")+1, url.lastIndexOf(".")-url.lastIndexOf("/")-1);
        jQuery('#this_url').html('<strong>' + pname + '</strong>');
        jQuery('body').attr('id', pname);
    });
</script>
<body>
    <div id="shark_old">
        <div id="wrap">
            <div id="header">
                <div id="site-name"><a href="../../sphinx_pages/build/html/index.html">Shark machine learning library</a></div>
                <ul id="nav">
                    <li >
                        <a href="../../sphinx_pages/build/html/rest_sources/installation.html">Installation</a>
                    </li>
		    <li >
                        <a href="../../sphinx_pages/build/html/rest_sources/tutorials/tutorials.html">Tutorials</a>
                    </li>
		    <li >
                        <a href="../../sphinx_pages/build/html/rest_sources/benchmark.html">Benchmarks</a>
                    </li>
                    <li class="active">
                        <a href="classes.html">Documentation</a>
                        <ul>
                            <li class="first"></li>
                            <li><a href="../../sphinx_pages/build/html/rest_sources/quickref/quickref.html">Quick references</a></li>
                            <li><a href="classes.html">Class list</a></li>
                            <li class="last"><a href="group__shark__globals.html">Global functions</a></li>
                        </ul>
                    </li>
                </ul>
            </div>
        </div>
    </div>
<div id="doxywrapper">
<!--
    <div id="global_doxytitle">Doxygen<br>Documentation:</div>
-->
    <div id="navrow_wrapper">
<!-- Generated by Doxygen 1.9.8 -->
<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_d44c64559bbebec7f509842c48db8b23.html">include</a></li><li class="navelem"><a class="el" href="dir_9d0c4981f10d03078bcfd5c74fe41ce8.html">shark</a></li><li class="navelem"><a class="el" href="dir_1791956e1c2f180e4a35bcf03083ac8e.html">ObjectiveFunctions</a></li><li class="navelem"><a class="el" href="dir_59485a58c0f22fabcd11143b4edd9c36.html">Benchmarks</a></li><li class="navelem"><a class="el" href="dir_92d843e43c0135636686ecde495d2b02.html">PoleSimulators</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle"><div class="title">DoublePole.h</div></div>
</div><!--header-->
<div class="contents">
<a href="_double_pole_8h.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a id="l00001" name="l00001"></a><span class="lineno">    1</span><span class="comment">/*!</span></div>
<div class="line"><a id="l00002" name="l00002"></a><span class="lineno">    2</span><span class="comment"> *</span></div>
<div class="line"><a id="l00003" name="l00003"></a><span class="lineno">    3</span><span class="comment"> * \brief Pole balancing simulation for double poles</span></div>
<div class="line"><a id="l00004" name="l00004"></a><span class="lineno">    4</span><span class="comment"> *</span></div>
<div class="line"><a id="l00005" name="l00005"></a><span class="lineno">    5</span><span class="comment"> * Class for simulating two poles balancing on a cart.</span></div>
<div class="line"><a id="l00006" name="l00006"></a><span class="lineno">    6</span><span class="comment"> * Based on code written by Verena Heidrich-Meisner for the paper</span></div>
<div class="line"><a id="l00007" name="l00007"></a><span class="lineno">    7</span><span class="comment"> *</span></div>
<div class="line"><a id="l00008" name="l00008"></a><span class="lineno">    8</span><span class="comment"> * V. Heidrich-Meisner and C. Igel. Neuroevolution strategies for episodic reinforcement learning. Journal of Algorithms, 64(4):152–168, 2009.</span></div>
<div class="line"><a id="l00009" name="l00009"></a><span class="lineno">    9</span><span class="comment"> *</span></div>
<div class="line"><a id="l00010" name="l00010"></a><span class="lineno">   10</span><span class="comment"> * which was in turn based on code available at http://webdocs.cs.ualberta.ca/~sutton/book/code/pole.c</span></div>
<div class="line"><a id="l00011" name="l00011"></a><span class="lineno">   11</span><span class="comment"> * as of 2015/4/19, written by Rich Sutton and Chuck Anderson and later modified.</span></div>
<div class="line"><a id="l00012" name="l00012"></a><span class="lineno">   12</span><span class="comment"> * Faustino Gomez wrote the physics code using the differential equations from</span></div>
<div class="line"><a id="l00013" name="l00013"></a><span class="lineno">   13</span><span class="comment"> * Alexis Weiland&#39;s paper and added the Runge-Kutta solver.</span></div>
<div class="line"><a id="l00014" name="l00014"></a><span class="lineno">   14</span><span class="comment"> *</span></div>
<div class="line"><a id="l00015" name="l00015"></a><span class="lineno">   15</span><span class="comment"> * \author      Johan Valentin Damgaard</span></div>
<div class="line"><a id="l00016" name="l00016"></a><span class="lineno">   16</span><span class="comment"> * \date        -</span></div>
<div class="line"><a id="l00017" name="l00017"></a><span class="lineno">   17</span><span class="comment"> *</span></div>
<div class="line"><a id="l00018" name="l00018"></a><span class="lineno">   18</span><span class="comment"> *</span></div>
<div class="line"><a id="l00019" name="l00019"></a><span class="lineno">   19</span><span class="comment"> * \par Copyright 1995-2017 Shark Development Team</span></div>
<div class="line"><a id="l00020" name="l00020"></a><span class="lineno">   20</span><span class="comment"> *</span></div>
<div class="line"><a id="l00021" name="l00021"></a><span class="lineno">   21</span><span class="comment"> * This file is part of Shark.</span></div>
<div class="line"><a id="l00022" name="l00022"></a><span class="lineno">   22</span><span class="comment"> * &lt;https://shark-ml.github.io/Shark/&gt;</span></div>
<div class="line"><a id="l00023" name="l00023"></a><span class="lineno">   23</span><span class="comment"> *</span></div>
<div class="line"><a id="l00024" name="l00024"></a><span class="lineno">   24</span><span class="comment"> * Shark is free software: you can redistribute it and/or modify</span></div>
<div class="line"><a id="l00025" name="l00025"></a><span class="lineno">   25</span><span class="comment"> * it under the terms of the GNU Lesser General Public License as published</span></div>
<div class="line"><a id="l00026" name="l00026"></a><span class="lineno">   26</span><span class="comment"> * by the Free Software Foundation, either version 3 of the License, or</span></div>
<div class="line"><a id="l00027" name="l00027"></a><span class="lineno">   27</span><span class="comment"> * (at your option) any later version.</span></div>
<div class="line"><a id="l00028" name="l00028"></a><span class="lineno">   28</span><span class="comment"> *</span></div>
<div class="line"><a id="l00029" name="l00029"></a><span class="lineno">   29</span><span class="comment"> * Shark is distributed in the hope that it will be useful,</span></div>
<div class="line"><a id="l00030" name="l00030"></a><span class="lineno">   30</span><span class="comment"> * but WITHOUT ANY WARRANTY; without even the implied warranty of</span></div>
<div class="line"><a id="l00031" name="l00031"></a><span class="lineno">   31</span><span class="comment"> * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the</span></div>
<div class="line"><a id="l00032" name="l00032"></a><span class="lineno">   32</span><span class="comment"> * GNU Lesser General Public License for more details.</span></div>
<div class="line"><a id="l00033" name="l00033"></a><span class="lineno">   33</span><span class="comment"> *</span></div>
<div class="line"><a id="l00034" name="l00034"></a><span class="lineno">   34</span><span class="comment"> * You should have received a copy of the GNU Lesser General Public License</span></div>
<div class="line"><a id="l00035" name="l00035"></a><span class="lineno">   35</span><span class="comment"> * along with Shark.  If not, see &lt;http://www.gnu.org/licenses/&gt;.</span></div>
<div class="line"><a id="l00036" name="l00036"></a><span class="lineno">   36</span><span class="comment"> *</span></div>
<div class="line"><a id="l00037" name="l00037"></a><span class="lineno">   37</span><span class="comment"> */</span></div>
<div class="line"><a id="l00038" name="l00038"></a><span class="lineno">   38</span><span class="preprocessor">#ifndef SHARK_OBJECTIVEFUNCTIONS_BENCHMARKS_POLE_TEXAS</span></div>
<div class="line"><a id="l00039" name="l00039"></a><span class="lineno">   39</span><span class="preprocessor">#define SHARK_OBJECTIVEFUNCTIONS_BENCHMARKS_POLE_TEXAS</span></div>
<div class="line"><a id="l00040" name="l00040"></a><span class="lineno">   40</span> </div>
<div class="line"><a id="l00041" name="l00041"></a><span class="lineno">   41</span><span class="preprocessor">#include &lt;<a class="code" href="_math_8h.html">math.h</a>&gt;</span></div>
<div class="line"><a id="l00042" name="l00042"></a><span class="lineno">   42</span><span class="preprocessor">#include &lt;<a class="code" href="_base_8h.html">shark/LinAlg/Base.h</a>&gt;</span></div>
<div class="line"><a id="l00043" name="l00043"></a><span class="lineno">   43</span> </div>
<div class="line"><a id="l00044" name="l00044"></a><span class="lineno">   44</span> </div>
<div class="line"><a id="l00045" name="l00045"></a><span class="lineno">   45</span> </div>
<div class="line"><a id="l00046" name="l00046"></a><span class="lineno">   46</span><span class="keyword">namespace </span><a class="code hl_namespace" href="namespaceshark.html" title="AbstractMultiObjectiveOptimizer.">shark</a> {</div>
<div class="line"><a id="l00047" name="l00047"></a><span class="lineno">   47</span> </div>
<div class="foldopen" id="foldopen00048" data-start="{" data-end="};">
<div class="line"><a id="l00048" name="l00048"></a><span class="lineno"><a class="line" href="classshark_1_1_double_pole.html">   48</a></span><span class="keyword">class </span><a class="code hl_class" href="classshark_1_1_double_pole.html">DoublePole</a> {</div>
<div class="line"><a id="l00049" name="l00049"></a><span class="lineno">   49</span><span class="keyword">public</span>:</div>
<div class="line"><a id="l00050" name="l00050"></a><span class="lineno">   50</span><span class="comment"></span> </div>
<div class="line"><a id="l00051" name="l00051"></a><span class="lineno">   51</span><span class="comment">    //! Number of variables</span></div>
<div class="foldopen" id="foldopen00052" data-start="{" data-end="}">
<div class="line"><a id="l00052" name="l00052"></a><span class="lineno"><a class="line" href="classshark_1_1_double_pole.html#a623d1c62e55c3df3a1c7219cefac3940">   52</a></span><span class="comment"></span>    <span class="keywordtype">unsigned</span> <a class="code hl_function" href="classshark_1_1_double_pole.html#a623d1c62e55c3df3a1c7219cefac3940" title="Number of variables.">noVars</a>()<span class="keyword"> const </span>{</div>
<div class="line"><a id="l00053" name="l00053"></a><span class="lineno">   53</span>        <span class="keywordflow">if</span>(m_isMarkovian)</div>
<div class="line"><a id="l00054" name="l00054"></a><span class="lineno">   54</span>            <span class="keywordflow">return</span> 6;</div>
<div class="line"><a id="l00055" name="l00055"></a><span class="lineno">   55</span>        <span class="keywordflow">return</span> 3;</div>
<div class="line"><a id="l00056" name="l00056"></a><span class="lineno">   56</span>    }</div>
</div>
<div class="line"><a id="l00057" name="l00057"></a><span class="lineno">   57</span><span class="comment"></span> </div>
<div class="line"><a id="l00058" name="l00058"></a><span class="lineno">   58</span><span class="comment">    //! \param markovian Whether to return velocities in getState</span></div>
<div class="line"><a id="l00059" name="l00059"></a><span class="lineno">   59</span><span class="comment">    //! \param normalize Whether to normalize return values in getState</span></div>
<div class="foldopen" id="foldopen00060" data-start="{" data-end="}">
<div class="line"><a id="l00060" name="l00060"></a><span class="lineno"><a class="line" href="classshark_1_1_double_pole.html#a7ab1e7c4c87e91b6a435473488b24b94">   60</a></span><span class="comment"></span>    <a class="code hl_function" href="classshark_1_1_double_pole.html#a7ab1e7c4c87e91b6a435473488b24b94">DoublePole</a>(<span class="keywordtype">bool</span> markovian, <span class="keywordtype">bool</span> normalize = <span class="keyword">true</span>) {</div>
<div class="line"><a id="l00061" name="l00061"></a><span class="lineno">   61</span>        m_isMarkovian = markovian;</div>
<div class="line"><a id="l00062" name="l00062"></a><span class="lineno">   62</span> </div>
<div class="line"><a id="l00063" name="l00063"></a><span class="lineno">   63</span>        <span class="comment">// current normalization constants are the same as used</span></div>
<div class="line"><a id="l00064" name="l00064"></a><span class="lineno">   64</span>        <span class="comment">// in the Heidrich-Meisner code</span></div>
<div class="line"><a id="l00065" name="l00065"></a><span class="lineno">   65</span>        <span class="comment">// perhaps tweak them?</span></div>
<div class="line"><a id="l00066" name="l00066"></a><span class="lineno">   66</span>        <span class="keywordflow">if</span>(normalize) {</div>
<div class="line"><a id="l00067" name="l00067"></a><span class="lineno">   67</span>            this-&gt;m_normal_cart = 4.8;</div>
<div class="line"><a id="l00068" name="l00068"></a><span class="lineno">   68</span>            this-&gt;m_normal_pole = 0.52;</div>
<div class="line"><a id="l00069" name="l00069"></a><span class="lineno">   69</span>            this-&gt;m_normal_velo = 2;</div>
<div class="line"><a id="l00070" name="l00070"></a><span class="lineno">   70</span>        }</div>
<div class="line"><a id="l00071" name="l00071"></a><span class="lineno">   71</span>        <span class="keywordflow">else</span> {</div>
<div class="line"><a id="l00072" name="l00072"></a><span class="lineno">   72</span>            this-&gt;m_normal_cart = 1.;</div>
<div class="line"><a id="l00073" name="l00073"></a><span class="lineno">   73</span>            this-&gt;m_normal_pole = 1.;</div>
<div class="line"><a id="l00074" name="l00074"></a><span class="lineno">   74</span>            this-&gt;m_normal_velo = 1.;</div>
<div class="line"><a id="l00075" name="l00075"></a><span class="lineno">   75</span>        }</div>
<div class="line"><a id="l00076" name="l00076"></a><span class="lineno">   76</span> </div>
<div class="line"><a id="l00077" name="l00077"></a><span class="lineno">   77</span>    }</div>
</div>
<div class="line"><a id="l00078" name="l00078"></a><span class="lineno">   78</span> </div>
<div class="line"><a id="l00079" name="l00079"></a><span class="lineno">   79</span><span class="comment"></span> </div>
<div class="line"><a id="l00080" name="l00080"></a><span class="lineno">   80</span><span class="comment">    //! \brief Place m_state in a vector</span></div>
<div class="line"><a id="l00081" name="l00081"></a><span class="lineno">   81</span><span class="comment">    //! \param v vector to place m_state in (assumed to be correct size already)</span></div>
<div class="foldopen" id="foldopen00082" data-start="{" data-end="}">
<div class="line"><a id="l00082" name="l00082"></a><span class="lineno"><a class="line" href="classshark_1_1_double_pole.html#a70cf32c8d658c4b6d55fc229c2e9226e">   82</a></span><span class="comment"></span>    <span class="keywordtype">void</span> <a class="code hl_function" href="classshark_1_1_double_pole.html#a70cf32c8d658c4b6d55fc229c2e9226e" title="Place m_state in a vector.">getState</a>(RealVector &amp;v) {</div>
<div class="line"><a id="l00083" name="l00083"></a><span class="lineno">   83</span>        <span class="comment">// normalize the m_state variables.</span></div>
<div class="line"><a id="l00084" name="l00084"></a><span class="lineno">   84</span>        <span class="keywordflow">if</span>(m_isMarkovian) {</div>
<div class="line"><a id="l00085" name="l00085"></a><span class="lineno">   85</span>            v(0) = m_state[0] / m_normal_cart;</div>
<div class="line"><a id="l00086" name="l00086"></a><span class="lineno">   86</span>            v(1) = m_state[1] / m_normal_velo;</div>
<div class="line"><a id="l00087" name="l00087"></a><span class="lineno">   87</span>            v(2) = m_state[2] / m_normal_pole;</div>
<div class="line"><a id="l00088" name="l00088"></a><span class="lineno">   88</span>            v(3) = m_state[3] / m_normal_velo;</div>
<div class="line"><a id="l00089" name="l00089"></a><span class="lineno">   89</span>            v(4) = m_state[4] / m_normal_pole;</div>
<div class="line"><a id="l00090" name="l00090"></a><span class="lineno">   90</span>            v(5) = m_state[5] / m_normal_velo;</div>
<div class="line"><a id="l00091" name="l00091"></a><span class="lineno">   91</span>        }</div>
<div class="line"><a id="l00092" name="l00092"></a><span class="lineno">   92</span>        <span class="keywordflow">else</span> {</div>
<div class="line"><a id="l00093" name="l00093"></a><span class="lineno">   93</span>            v(0) = m_state[0] / m_normal_cart;</div>
<div class="line"><a id="l00094" name="l00094"></a><span class="lineno">   94</span>            v(1) = m_state[2] / m_normal_pole;</div>
<div class="line"><a id="l00095" name="l00095"></a><span class="lineno">   95</span>            v(2) = m_state[4] / m_normal_pole;</div>
<div class="line"><a id="l00096" name="l00096"></a><span class="lineno">   96</span>        }</div>
<div class="line"><a id="l00097" name="l00097"></a><span class="lineno">   97</span>    }</div>
</div>
<div class="line"><a id="l00098" name="l00098"></a><span class="lineno">   98</span><span class="comment"></span> </div>
<div class="line"><a id="l00099" name="l00099"></a><span class="lineno">   99</span><span class="comment">    //! \brief Initialize with specific angle for large pole</span></div>
<div class="line"><a id="l00100" name="l00100"></a><span class="lineno">  100</span><span class="comment">    //! \param state2init initial pole angle (in radians)</span></div>
<div class="foldopen" id="foldopen00101" data-start="{" data-end="}">
<div class="line"><a id="l00101" name="l00101"></a><span class="lineno"><a class="line" href="classshark_1_1_double_pole.html#a3b14136fc5153dcd09160d81a432fa81">  101</a></span><span class="comment"></span>    <span class="keywordtype">void</span> <a class="code hl_function" href="classshark_1_1_double_pole.html#a3b14136fc5153dcd09160d81a432fa81" title="Initialize with specific angle for large pole.">init</a>(<span class="keywordtype">double</span> state2init = 0.07)  {</div>
<div class="line"><a id="l00102" name="l00102"></a><span class="lineno">  102</span>        m_state[0] = m_state[1] = m_state[3] = m_state[4] = m_state[5] = 0;</div>
<div class="line"><a id="l00103" name="l00103"></a><span class="lineno">  103</span>        m_state[2] = state2init;</div>
<div class="line"><a id="l00104" name="l00104"></a><span class="lineno">  104</span>    }</div>
</div>
<div class="line"><a id="l00105" name="l00105"></a><span class="lineno">  105</span><span class="comment"></span> </div>
<div class="line"><a id="l00106" name="l00106"></a><span class="lineno">  106</span><span class="comment">    //! \brief Initialize with specific angle for large pole</span></div>
<div class="line"><a id="l00107" name="l00107"></a><span class="lineno">  107</span><span class="comment">    //! \param state2init initial pole angle (in degrees)</span></div>
<div class="foldopen" id="foldopen00108" data-start="{" data-end="}">
<div class="line"><a id="l00108" name="l00108"></a><span class="lineno"><a class="line" href="classshark_1_1_double_pole.html#a89b7eaa944fae019602a677a834eaba8">  108</a></span><span class="comment"></span>    <span class="keywordtype">void</span> <a class="code hl_function" href="classshark_1_1_double_pole.html#a89b7eaa944fae019602a677a834eaba8" title="Initialize with specific angle for large pole.">initDegree</a>(<span class="keywordtype">double</span> state2init)  {</div>
<div class="line"><a id="l00109" name="l00109"></a><span class="lineno">  109</span>        <a class="code hl_function" href="classshark_1_1_double_pole.html#a3b14136fc5153dcd09160d81a432fa81" title="Initialize with specific angle for large pole.">init</a>(state2init * M_PI / 180);</div>
<div class="line"><a id="l00110" name="l00110"></a><span class="lineno">  110</span>    }</div>
</div>
<div class="line"><a id="l00111" name="l00111"></a><span class="lineno">  111</span><span class="comment"></span> </div>
<div class="line"><a id="l00112" name="l00112"></a><span class="lineno">  112</span><span class="comment">    //! \brief Initialize full m_state</span></div>
<div class="line"><a id="l00113" name="l00113"></a><span class="lineno">  113</span><span class="comment">    //! \param a initial cart position</span></div>
<div class="line"><a id="l00114" name="l00114"></a><span class="lineno">  114</span><span class="comment">    //! \param b initial cart velocity</span></div>
<div class="line"><a id="l00115" name="l00115"></a><span class="lineno">  115</span><span class="comment">    //! \param c initial large pole angle (in radians)</span></div>
<div class="line"><a id="l00116" name="l00116"></a><span class="lineno">  116</span><span class="comment">    //! \param d initial large pole angular velocity</span></div>
<div class="line"><a id="l00117" name="l00117"></a><span class="lineno">  117</span><span class="comment">    //! \param e initial small pole angle (in radians)</span></div>
<div class="line"><a id="l00118" name="l00118"></a><span class="lineno">  118</span><span class="comment">    //! \param f initial small pole angular velocity</span></div>
<div class="foldopen" id="foldopen00119" data-start="{" data-end="}">
<div class="line"><a id="l00119" name="l00119"></a><span class="lineno"><a class="line" href="classshark_1_1_double_pole.html#a0ab0da8d67c1bdd2987615a1fb25a3f6">  119</a></span><span class="comment"></span>    <span class="keywordtype">void</span> <a class="code hl_function" href="classshark_1_1_double_pole.html#a0ab0da8d67c1bdd2987615a1fb25a3f6" title="Initialize full m_state.">init</a>(<span class="keywordtype">double</span> a, <span class="keywordtype">double</span> b, <span class="keywordtype">double</span> c, <span class="keywordtype">double</span> d, <span class="keywordtype">double</span> e = 0., <span class="keywordtype">double</span> f = 0.)  {</div>
<div class="line"><a id="l00120" name="l00120"></a><span class="lineno">  120</span>        m_state[0] = a;</div>
<div class="line"><a id="l00121" name="l00121"></a><span class="lineno">  121</span>        m_state[1] = b;</div>
<div class="line"><a id="l00122" name="l00122"></a><span class="lineno">  122</span>        m_state[2] = c;</div>
<div class="line"><a id="l00123" name="l00123"></a><span class="lineno">  123</span>        m_state[3] = d;</div>
<div class="line"><a id="l00124" name="l00124"></a><span class="lineno">  124</span>        m_state[4] = e;</div>
<div class="line"><a id="l00125" name="l00125"></a><span class="lineno">  125</span>        m_state[5] = f;</div>
<div class="line"><a id="l00126" name="l00126"></a><span class="lineno">  126</span>    }</div>
</div>
<div class="line"><a id="l00127" name="l00127"></a><span class="lineno">  127</span><span class="comment"></span> </div>
<div class="line"><a id="l00128" name="l00128"></a><span class="lineno">  128</span><span class="comment">    //! Returns true when this pole is in an illegal position</span></div>
<div class="foldopen" id="foldopen00129" data-start="{" data-end="}">
<div class="line"><a id="l00129" name="l00129"></a><span class="lineno"><a class="line" href="classshark_1_1_double_pole.html#ac24a26634511052a94f55986507ed70a">  129</a></span><span class="comment"></span>    <span class="keywordtype">bool</span> <a class="code hl_function" href="classshark_1_1_double_pole.html#ac24a26634511052a94f55986507ed70a" title="Returns true when this pole is in an illegal position.">failure</a>()</div>
<div class="line"><a id="l00130" name="l00130"></a><span class="lineno">  130</span>    {</div>
<div class="line"><a id="l00131" name="l00131"></a><span class="lineno">  131</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> thirty_six_degrees= 0.628329;</div>
<div class="line"><a id="l00132" name="l00132"></a><span class="lineno">  132</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> failureAngle = thirty_six_degrees;</div>
<div class="line"><a id="l00133" name="l00133"></a><span class="lineno">  133</span> </div>
<div class="line"><a id="l00134" name="l00134"></a><span class="lineno">  134</span>        <span class="keywordflow">return</span>(m_state[0] &lt; -2.4              ||</div>
<div class="line"><a id="l00135" name="l00135"></a><span class="lineno">  135</span>               m_state[0] &gt; 2.4               ||</div>
<div class="line"><a id="l00136" name="l00136"></a><span class="lineno">  136</span>               m_state[2] &lt; -failureAngle     ||</div>
<div class="line"><a id="l00137" name="l00137"></a><span class="lineno">  137</span>               m_state[2] &gt; failureAngle      ||</div>
<div class="line"><a id="l00138" name="l00138"></a><span class="lineno">  138</span>               m_state[4] &lt; -failureAngle     ||</div>
<div class="line"><a id="l00139" name="l00139"></a><span class="lineno">  139</span>               m_state[4] &gt; failureAngle);</div>
<div class="line"><a id="l00140" name="l00140"></a><span class="lineno">  140</span>    }</div>
</div>
<div class="line"><a id="l00141" name="l00141"></a><span class="lineno">  141</span><span class="comment"></span> </div>
<div class="line"><a id="l00142" name="l00142"></a><span class="lineno">  142</span><span class="comment">    //! Return &quot;jiggle&quot;, abstract representation of how much the the cart oscillates</span></div>
<div class="foldopen" id="foldopen00143" data-start="{" data-end="}">
<div class="line"><a id="l00143" name="l00143"></a><span class="lineno"><a class="line" href="classshark_1_1_double_pole.html#a47aae9cea93635b74c06f27427258a1a">  143</a></span><span class="comment"></span>    <span class="keywordtype">double</span> <a class="code hl_function" href="classshark_1_1_double_pole.html#a47aae9cea93635b74c06f27427258a1a" title="Return &quot;jiggle&quot;, abstract representation of how much the the cart oscillates.">getJiggle</a>() {</div>
<div class="line"><a id="l00144" name="l00144"></a><span class="lineno">  144</span>        <span class="keywordflow">return</span> (std::abs(m_state[0]) + std::abs(m_state[1]) + std::abs(m_state[2]) + std::abs(m_state[3]));</div>
<div class="line"><a id="l00145" name="l00145"></a><span class="lineno">  145</span>    }</div>
</div>
<div class="line"><a id="l00146" name="l00146"></a><span class="lineno">  146</span><span class="comment"></span> </div>
<div class="line"><a id="l00147" name="l00147"></a><span class="lineno">  147</span><span class="comment">    //! \brief Move the pole with some force</span></div>
<div class="line"><a id="l00148" name="l00148"></a><span class="lineno">  148</span><span class="comment">    //! \param output Force to apply. Expects values in [0,1], where values below 0.5 indicate applying force towards the left side and values above indicate force towards the right.</span></div>
<div class="foldopen" id="foldopen00149" data-start="{" data-end="}">
<div class="line"><a id="l00149" name="l00149"></a><span class="lineno"><a class="line" href="classshark_1_1_double_pole.html#a95065838bc7f6ceb3fa85ae568bc9def">  149</a></span><span class="comment"></span>    <span class="keywordtype">void</span> <a class="code hl_function" href="classshark_1_1_double_pole.html#a95065838bc7f6ceb3fa85ae568bc9def" title="Move the pole with some force.">move</a>(<span class="keywordtype">double</span> output)</div>
<div class="line"><a id="l00150" name="l00150"></a><span class="lineno">  150</span>    {</div>
<div class="line"><a id="l00151" name="l00151"></a><span class="lineno">  151</span>        <span class="keywordtype">double</span>  dydx[6];</div>
<div class="line"><a id="l00152" name="l00152"></a><span class="lineno">  152</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> TAU= 0.01;</div>
<div class="line"><a id="l00153" name="l00153"></a><span class="lineno">  153</span> </div>
<div class="line"><a id="l00154" name="l00154"></a><span class="lineno">  154</span>        <span class="keyword">const</span> <span class="keywordtype">bool</span> useRK4=<span class="keyword">true</span>;</div>
<div class="line"><a id="l00155" name="l00155"></a><span class="lineno">  155</span>        <span class="keywordflow">if</span>(useRK4) {</div>
<div class="line"><a id="l00156" name="l00156"></a><span class="lineno">  156</span>            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;2; ++i) {</div>
<div class="line"><a id="l00157" name="l00157"></a><span class="lineno">  157</span>                dydx[0] = m_state[1];</div>
<div class="line"><a id="l00158" name="l00158"></a><span class="lineno">  158</span>                dydx[2] = m_state[3];</div>
<div class="line"><a id="l00159" name="l00159"></a><span class="lineno">  159</span>                dydx[4] = m_state[5];</div>
<div class="line"><a id="l00160" name="l00160"></a><span class="lineno">  160</span>                step(output,m_state,dydx);</div>
<div class="line"><a id="l00161" name="l00161"></a><span class="lineno">  161</span>                rk4(output,m_state,dydx,m_state);</div>
<div class="line"><a id="l00162" name="l00162"></a><span class="lineno">  162</span>            }</div>
<div class="line"><a id="l00163" name="l00163"></a><span class="lineno">  163</span>        }</div>
<div class="line"><a id="l00164" name="l00164"></a><span class="lineno">  164</span>        <span class="keywordflow">else</span> {</div>
<div class="line"><a id="l00165" name="l00165"></a><span class="lineno">  165</span>            <span class="keyword">const</span> <span class="keywordtype">double</span> EULER_STEPS = 4.;</div>
<div class="line"><a id="l00166" name="l00166"></a><span class="lineno">  166</span>            <span class="keyword">const</span> <span class="keywordtype">double</span> EULER_TAU= TAU/EULER_STEPS;</div>
<div class="line"><a id="l00167" name="l00167"></a><span class="lineno">  167</span>            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;2 * EULER_STEPS; ++i) {</div>
<div class="line"><a id="l00168" name="l00168"></a><span class="lineno">  168</span>                step(output,m_state,dydx);</div>
<div class="line"><a id="l00169" name="l00169"></a><span class="lineno">  169</span>                m_state[0] += EULER_TAU * m_state[1];</div>
<div class="line"><a id="l00170" name="l00170"></a><span class="lineno">  170</span>                m_state[1] += EULER_TAU * dydx[1];</div>
<div class="line"><a id="l00171" name="l00171"></a><span class="lineno">  171</span>                m_state[2] += EULER_TAU * m_state[3];</div>
<div class="line"><a id="l00172" name="l00172"></a><span class="lineno">  172</span>                m_state[3] += EULER_TAU * dydx[3];</div>
<div class="line"><a id="l00173" name="l00173"></a><span class="lineno">  173</span>                m_state[4] += EULER_TAU * m_state[5];</div>
<div class="line"><a id="l00174" name="l00174"></a><span class="lineno">  174</span>                m_state[5] += EULER_TAU * dydx[5];</div>
<div class="line"><a id="l00175" name="l00175"></a><span class="lineno">  175</span>            }</div>
<div class="line"><a id="l00176" name="l00176"></a><span class="lineno">  176</span>        }</div>
<div class="line"><a id="l00177" name="l00177"></a><span class="lineno">  177</span>    }</div>
</div>
<div class="line"><a id="l00178" name="l00178"></a><span class="lineno">  178</span> </div>
<div class="line"><a id="l00179" name="l00179"></a><span class="lineno">  179</span><span class="keyword">private</span>:</div>
<div class="line"><a id="l00180" name="l00180"></a><span class="lineno">  180</span>    <span class="keywordtype">void</span> step(<span class="keywordtype">double</span> action, <span class="keywordtype">double</span> *st, <span class="keywordtype">double</span> *derivs)</div>
<div class="line"><a id="l00181" name="l00181"></a><span class="lineno">  181</span>    {</div>
<div class="line"><a id="l00182" name="l00182"></a><span class="lineno">  182</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> MUP = 0.000002;</div>
<div class="line"><a id="l00183" name="l00183"></a><span class="lineno">  183</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> MUC = 0.0005;</div>
<div class="line"><a id="l00184" name="l00184"></a><span class="lineno">  184</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> GRAVITY= -9.8;</div>
<div class="line"><a id="l00185" name="l00185"></a><span class="lineno">  185</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> MASSCART= 1.0;</div>
<div class="line"><a id="l00186" name="l00186"></a><span class="lineno">  186</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> LENGTH_1   = 0.5;</div>
<div class="line"><a id="l00187" name="l00187"></a><span class="lineno">  187</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> MASSPOLE_1 = 0.1;</div>
<div class="line"><a id="l00188" name="l00188"></a><span class="lineno">  188</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> LENGTH_2   = 0.1 * LENGTH_1;</div>
<div class="line"><a id="l00189" name="l00189"></a><span class="lineno">  189</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> MASSPOLE_2 = 0.1 * MASSPOLE_1;</div>
<div class="line"><a id="l00190" name="l00190"></a><span class="lineno">  190</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> FORCE_MAG= 10.0;</div>
<div class="line"><a id="l00191" name="l00191"></a><span class="lineno">  191</span> </div>
<div class="line"><a id="l00192" name="l00192"></a><span class="lineno">  192</span>        <span class="keywordtype">double</span> signum = 0.;</div>
<div class="line"><a id="l00193" name="l00193"></a><span class="lineno">  193</span> </div>
<div class="line"><a id="l00194" name="l00194"></a><span class="lineno">  194</span>        <span class="keywordflow">if</span> (st[1] &lt; 0) {</div>
<div class="line"><a id="l00195" name="l00195"></a><span class="lineno">  195</span>            signum = -1;</div>
<div class="line"><a id="l00196" name="l00196"></a><span class="lineno">  196</span>        }</div>
<div class="line"><a id="l00197" name="l00197"></a><span class="lineno">  197</span>        <span class="keywordflow">if</span> (st[1] &gt; 0) {</div>
<div class="line"><a id="l00198" name="l00198"></a><span class="lineno">  198</span>            signum = 1;</div>
<div class="line"><a id="l00199" name="l00199"></a><span class="lineno">  199</span>        }</div>
<div class="line"><a id="l00200" name="l00200"></a><span class="lineno">  200</span> </div>
<div class="line"><a id="l00201" name="l00201"></a><span class="lineno">  201</span>        <span class="keywordtype">double</span> force =  ((action - 0.5) * FORCE_MAG * 2) - (MUC*signum);</div>
<div class="line"><a id="l00202" name="l00202"></a><span class="lineno">  202</span>        <span class="keywordtype">double</span> costheta_1 = cos(st[2]);</div>
<div class="line"><a id="l00203" name="l00203"></a><span class="lineno">  203</span>        <span class="keywordtype">double</span> sintheta_1 = sin(st[2]);</div>
<div class="line"><a id="l00204" name="l00204"></a><span class="lineno">  204</span>        <span class="keywordtype">double</span> gsintheta_1 = GRAVITY * sintheta_1;</div>
<div class="line"><a id="l00205" name="l00205"></a><span class="lineno">  205</span>        <span class="keywordtype">double</span> costheta_2 = cos(st[4]);</div>
<div class="line"><a id="l00206" name="l00206"></a><span class="lineno">  206</span>        <span class="keywordtype">double</span> sintheta_2 = sin(st[4]);</div>
<div class="line"><a id="l00207" name="l00207"></a><span class="lineno">  207</span>        <span class="keywordtype">double</span> gsintheta_2 = GRAVITY * sintheta_2;</div>
<div class="line"><a id="l00208" name="l00208"></a><span class="lineno">  208</span> </div>
<div class="line"><a id="l00209" name="l00209"></a><span class="lineno">  209</span>        <span class="keywordtype">double</span> ml_1 = LENGTH_1 * MASSPOLE_1;</div>
<div class="line"><a id="l00210" name="l00210"></a><span class="lineno">  210</span>        <span class="keywordtype">double</span> ml_2 = LENGTH_2 * MASSPOLE_2;</div>
<div class="line"><a id="l00211" name="l00211"></a><span class="lineno">  211</span>        <span class="keywordtype">double</span> temp_1 = MUP * st[3] / ml_1;</div>
<div class="line"><a id="l00212" name="l00212"></a><span class="lineno">  212</span>        <span class="keywordtype">double</span> temp_2 = MUP * st[5] / ml_2;</div>
<div class="line"><a id="l00213" name="l00213"></a><span class="lineno">  213</span>        <span class="keywordtype">double</span> fi_1 = (ml_1 * st[3] * st[3] * sintheta_1) +</div>
<div class="line"><a id="l00214" name="l00214"></a><span class="lineno">  214</span>               (0.75 * MASSPOLE_1 * costheta_1 * (temp_1 + gsintheta_1));</div>
<div class="line"><a id="l00215" name="l00215"></a><span class="lineno">  215</span>        <span class="keywordtype">double</span> fi_2 = (ml_2 * st[5] * st[5] * sintheta_2) +</div>
<div class="line"><a id="l00216" name="l00216"></a><span class="lineno">  216</span>               (0.75 * MASSPOLE_2 * costheta_2 * (temp_2 + gsintheta_2));</div>
<div class="line"><a id="l00217" name="l00217"></a><span class="lineno">  217</span>        <span class="keywordtype">double</span> mi_1 = MASSPOLE_1 * (1 - (0.75 * costheta_1 * costheta_1));</div>
<div class="line"><a id="l00218" name="l00218"></a><span class="lineno">  218</span>        <span class="keywordtype">double</span> mi_2 = MASSPOLE_2 * (1 - (0.75 * costheta_2 * costheta_2));</div>
<div class="line"><a id="l00219" name="l00219"></a><span class="lineno">  219</span> </div>
<div class="line"><a id="l00220" name="l00220"></a><span class="lineno">  220</span>        derivs[1] = (force + fi_1 + fi_2)</div>
<div class="line"><a id="l00221" name="l00221"></a><span class="lineno">  221</span>                    / (mi_1 + mi_2 + MASSCART);</div>
<div class="line"><a id="l00222" name="l00222"></a><span class="lineno">  222</span> </div>
<div class="line"><a id="l00223" name="l00223"></a><span class="lineno">  223</span>        derivs[3] = -0.75 * (derivs[1] * costheta_1 + gsintheta_1 + temp_1)</div>
<div class="line"><a id="l00224" name="l00224"></a><span class="lineno">  224</span>                    / LENGTH_1;</div>
<div class="line"><a id="l00225" name="l00225"></a><span class="lineno">  225</span>        derivs[5] = -0.75 * (derivs[1] * costheta_2 + gsintheta_2 + temp_2)</div>
<div class="line"><a id="l00226" name="l00226"></a><span class="lineno">  226</span>                    / LENGTH_2;</div>
<div class="line"><a id="l00227" name="l00227"></a><span class="lineno">  227</span> </div>
<div class="line"><a id="l00228" name="l00228"></a><span class="lineno">  228</span>    }</div>
<div class="line"><a id="l00229" name="l00229"></a><span class="lineno">  229</span> </div>
<div class="line"><a id="l00230" name="l00230"></a><span class="lineno">  230</span>    <span class="keywordtype">void</span> rk4(<span class="keywordtype">double</span> f, <span class="keywordtype">double</span> y[], <span class="keywordtype">double</span> dydx[], <span class="keywordtype">double</span> yout[])</div>
<div class="line"><a id="l00231" name="l00231"></a><span class="lineno">  231</span>    {</div>
<div class="line"><a id="l00232" name="l00232"></a><span class="lineno">  232</span>        <span class="keyword">const</span> <span class="keywordtype">double</span> TAU= 0.01;</div>
<div class="line"><a id="l00233" name="l00233"></a><span class="lineno">  233</span>        <span class="keywordtype">double</span> dym[6],dyt[6],yt[6];</div>
<div class="line"><a id="l00234" name="l00234"></a><span class="lineno">  234</span> </div>
<div class="line"><a id="l00235" name="l00235"></a><span class="lineno">  235</span>        <span class="keywordtype">double</span> hh=TAU*0.5;</div>
<div class="line"><a id="l00236" name="l00236"></a><span class="lineno">  236</span>        <span class="keywordtype">double</span> h6=TAU/6.0;</div>
<div class="line"><a id="l00237" name="l00237"></a><span class="lineno">  237</span>        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=0; i&lt;=5; i++) yt[i]=y[i]+hh*dydx[i];</div>
<div class="line"><a id="l00238" name="l00238"></a><span class="lineno">  238</span>        step(f,yt,dyt);</div>
<div class="line"><a id="l00239" name="l00239"></a><span class="lineno">  239</span>        dyt[0] = yt[1];</div>
<div class="line"><a id="l00240" name="l00240"></a><span class="lineno">  240</span>        dyt[2] = yt[3];</div>
<div class="line"><a id="l00241" name="l00241"></a><span class="lineno">  241</span>        dyt[4] = yt[5];</div>
<div class="line"><a id="l00242" name="l00242"></a><span class="lineno">  242</span>        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=0; i&lt;=5; i++) yt[i]=y[i]+hh*dyt[i];</div>
<div class="line"><a id="l00243" name="l00243"></a><span class="lineno">  243</span>        step(f,yt,dym);</div>
<div class="line"><a id="l00244" name="l00244"></a><span class="lineno">  244</span>        dym[0] = yt[1];</div>
<div class="line"><a id="l00245" name="l00245"></a><span class="lineno">  245</span>        dym[2] = yt[3];</div>
<div class="line"><a id="l00246" name="l00246"></a><span class="lineno">  246</span>        dym[4] = yt[5];</div>
<div class="line"><a id="l00247" name="l00247"></a><span class="lineno">  247</span>        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=0; i&lt;=5; i++) {</div>
<div class="line"><a id="l00248" name="l00248"></a><span class="lineno">  248</span>            yt[i]=y[i]+TAU*dym[i];</div>
<div class="line"><a id="l00249" name="l00249"></a><span class="lineno">  249</span>            dym[i] += dyt[i];</div>
<div class="line"><a id="l00250" name="l00250"></a><span class="lineno">  250</span>        }</div>
<div class="line"><a id="l00251" name="l00251"></a><span class="lineno">  251</span>        step(f,yt,dyt);</div>
<div class="line"><a id="l00252" name="l00252"></a><span class="lineno">  252</span>        dyt[0] = yt[1];</div>
<div class="line"><a id="l00253" name="l00253"></a><span class="lineno">  253</span>        dyt[2] = yt[3];</div>
<div class="line"><a id="l00254" name="l00254"></a><span class="lineno">  254</span>        dyt[4] = yt[5];</div>
<div class="line"><a id="l00255" name="l00255"></a><span class="lineno">  255</span>        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=0; i&lt;=5; i++)</div>
<div class="line"><a id="l00256" name="l00256"></a><span class="lineno">  256</span>            yout[i]=y[i]+h6*(dydx[i]+dyt[i]+2.0*dym[i]);</div>
<div class="line"><a id="l00257" name="l00257"></a><span class="lineno">  257</span>    }</div>
<div class="line"><a id="l00258" name="l00258"></a><span class="lineno">  258</span> </div>
<div class="line"><a id="l00259" name="l00259"></a><span class="lineno">  259</span>    <span class="keywordtype">bool</span> m_isMarkovian;</div>
<div class="line"><a id="l00260" name="l00260"></a><span class="lineno">  260</span> </div>
<div class="line"><a id="l00261" name="l00261"></a><span class="lineno">  261</span>    <span class="keywordtype">double</span> m_state[6];</div>
<div class="line"><a id="l00262" name="l00262"></a><span class="lineno">  262</span> </div>
<div class="line"><a id="l00263" name="l00263"></a><span class="lineno">  263</span>    <span class="keywordtype">double</span> m_normal_cart;</div>
<div class="line"><a id="l00264" name="l00264"></a><span class="lineno">  264</span>    <span class="keywordtype">double</span> m_normal_pole;</div>
<div class="line"><a id="l00265" name="l00265"></a><span class="lineno">  265</span>    <span class="keywordtype">double</span> m_normal_velo;</div>
<div class="line"><a id="l00266" name="l00266"></a><span class="lineno">  266</span> </div>
<div class="line"><a id="l00267" name="l00267"></a><span class="lineno">  267</span>};</div>
</div>
<div class="line"><a id="l00268" name="l00268"></a><span class="lineno">  268</span>}</div>
<div class="line"><a id="l00269" name="l00269"></a><span class="lineno">  269</span><span class="preprocessor">#endif</span></div>
</div><!-- fragment --></div><!-- contents -->
</div>
</body>
</html>
